#include "lidar_main.h"

#include "C3iroboticsLidar.h"
#include "CSerialConnection.h"
#include <pthread.h>
#include <math.h>

#define DEG2RAD(x) ((x)*M_PI/180.)
#define UART_PART_NAME "/dev/ttyAMA1"
#define LIDARDISINF 10000

HiRobot::LaserScan data;
pthread_mutex_t dataLock;

bool isReady=false;

typedef struct _rslidar_data
{
    _rslidar_data()
    {
        signal = 0;
        angle = 0.0;
        distance = 0.0;
    }
    uint8_t signal;
    float   angle;
    float   distance;
}RslidarDataComplete;

using namespace std;
using namespace everest::hwdrivers;

CSerialConnection serial_connect;

void* readLidarThread(void*)
{
    int    opt_com_baudrate = 230400;
    string opt_com_path = UART_PART_NAME;


    C3iroboticsLidar robotics_lidar;

    pthread_mutex_init(&dataLock,NULL);

    serial_connect.setBaud(opt_com_baudrate);
    serial_connect.setPort(opt_com_path.c_str());
    if(serial_connect.openSimple())
    {
        printf("[AuxCtrl] Open serail port sucessful!\n");
        printf("baud rate:%d\n",serial_connect.getBaud());
    }
    else
    {
        printf("[AuxCtrl] Open serail port %s failed! \n", opt_com_path.c_str());
        return 0;
    }

    printf("C3iroboticslidar connected\n");

    robotics_lidar.initilize(&serial_connect);
    while(1)
    {
        TLidarGrabResult result = robotics_lidar.getScanData();
        switch(result)
        {
            case LIDAR_GRAB_ING:
            {
                break;
            }
            case LIDAR_GRAB_SUCESS:
            {
                TLidarScan lidar_scan = robotics_lidar.getLidarScan();
                size_t lidar_scan_size = lidar_scan.getSize();
                std::vector<RslidarDataComplete> send_lidar_scan_data;
                send_lidar_scan_data.resize(lidar_scan_size);
                RslidarDataComplete one_lidar_data;
                for(size_t i = 0; i < lidar_scan_size; i++)
                {
                    one_lidar_data.signal = lidar_scan.signal[i];
                    one_lidar_data.angle = lidar_scan.angle[i];
                    one_lidar_data.distance = lidar_scan.distance[i];
                    send_lidar_scan_data[i] = one_lidar_data;
                }

                // printf("Lidar count %d!\n", lidar_scan_size);
                
                pthread_mutex_lock(&dataLock);

                data.angle_min = DEG2RAD(0.0f);
                data.angle_max = DEG2RAD(359.0f);
                data.angle_increment = (data.angle_max - data.angle_min) / (360.0f - 1.0f);
                data.range_max=5.0;
                data.range_min=0.15;
                data.ranges.resize(360, LIDARDISINF);

                for (size_t i = 0; i < lidar_scan_size; i++)
                {
                    size_t current_angle = floor(send_lidar_scan_data[i].angle);
                    if(current_angle > 360.0)
                    {
                        printf("Lidar angle is out of range %d\n", (int)current_angle);
                        continue;
                    }
                    float read_value = (float) send_lidar_scan_data[i].distance;
                    if (read_value < data.range_min || read_value > data.range_max)
                        data.ranges[360- 1- current_angle] = LIDARDISINF;
                    else
                        data.ranges[360 -1- current_angle] = read_value;

                }
                isReady=true;
                pthread_mutex_unlock(&dataLock);
                break;
            }
            case LIDAR_GRAB_ERRO:
            {
                break;
            }
            case LIDAR_GRAB_ELSE:
            {
                // printf("[Main] LIDAR_GRAB_ELSE!\n");
                break;
            }
        }
    }
}

bool lidarDataIsReady()
{
    bool tmp;
    pthread_mutex_lock(&dataLock);
    tmp=isReady;
    pthread_mutex_unlock(&dataLock);
    return tmp;
}

HiRobot::LaserScan lidarGetData()
{
    HiRobot::LaserScan tmp;
    pthread_mutex_lock(&dataLock);
    tmp=data;
    isReady=false;
    pthread_mutex_unlock(&dataLock);
    return tmp;
}

void* getSerialPort()
{
    return (void*)&serial_connect;
}

